// GeometryFunctions.h: maths functions for the Geometry Class
// Created by Zane Moser and Camille Nicodemus
// Function implementation based on class specific implementation 
// by Project 5 Team (715), 2009
//////////////////////////////////////////////////////////////////////

#ifndef GEOMETRY_FUNCTIONS_H
#define GEOMETRY_FUNCTIONS_H

#include "Geometry.h"
#include <vector>

using namespace std;

template <typename T> double	distToLine(CVec2d<typename T>* p, vector<CVec2d<typename T>*>* line);
template <typename T> int		findFurtherestPoint(vector<CVec2d<typename T>*>* points);
template <typename T> bool		internalPoint(CVec2d<typename T>* point, CVec2d<typename T>* p1, CVec2d<typename T>* p2);
template <typename T> double	lineAngle(CVec2d<typename T>* p1, CVec2d<typename T>* p2);
template <typename T> int		lineFit(vector<CVec2d<typename T>*>* input, vector<CVec2d<typename T>*>* output);	
template <typename T> double	lineLength(CVec2d<typename T>* p1, CVec2d<typename T>* p2);

template <typename T> double	distToLine(CVec2d<typename T>* p, vector<CVec2d<typename T>*>* line)
{
	CVec2d<T> start = *line->front();
	CVec2d<T> end   = *line->back();
	
	double theta = lineAngle(&start, &end);
	double phi   = lineAngle(&start, p);
	double angle = phi-theta;
	
	double ret   = lineLength(p, &start)*sin(angle);
	if(ret < 0)
		ret *= -1;
	return ret;
}

template <typename T> int		findFurtherestPoint(vector<CVec2d<typename T>*>* points)
{
	int index = 0;
	double maxDist = 0;
	
	if(points->size() <=2)
		return 0;
	
	for(int i = 1; i < points->size() - 2; i++)
	{
		double d = distToLine((*points)[i], points);
		if(d > maxDist)
		{
			maxDist = d;
			index = i;
		}
	}
	
	if(maxDist > 7.0)
		return index;
	return 0;
}

template <typename T> bool		internalPoint(CVec2d<typename T>* point, CVec2d<typename T>* p1, CVec2d<typename T>* p2)
{
	double theta = lineAngle(p1,p2);
	double phi   = lineAngle(p1, point);
	double angle1 = fabs(phi-theta);
	if (angle1 > (Pi/2.0f) && angle1 < (3.0f*Pi/2.0f))
		return false;
	
	theta = lineAngle(p2, p1);
	phi	  = lineAngle(p2, point);
	double angle3 = fabs(phi-theta);
	if (angle3 > (Pi/2.0f) && angle3 < (3.0f*Pi/2.0f))
		return false;
	return true;
}

template <typename T> double	lineAngle(CVec2d<typename T>* p1, CVec2d<typename T>* p2)
{
	double x = (double)((*p1)[0] - (*p2)[0]);
	double y = (double)((*p1)[1] - (*p2)[1]);
	return atan2(y,x);
}

template <typename T> int		lineFit(vector<CVec2d<typename T>*>* input, vector<CVec2d<typename T>*>* output)
{
	if(input->size()<=1)
	{
		return -1;
	}
	
	output->push_back((*input)[0]);
	
	int maxPointIndex = findFurtherestPoint(input);
	
	if(maxPointIndex > 0)
	{
		//printf("Max point Index: %d\n",maxPointIndex);
		vector<CVec2di*> leftList;
		for(int i = 0; i< maxPointIndex; i++)
		{
			leftList.push_back((*input)[i]);
		}
		leftList.push_back((*input)[maxPointIndex]);
		vector<CVec2di*> rightList;
		for(int i = maxPointIndex; i< input->size() - 1; i++)
		{
			rightList.push_back((*input)[i]);
		}
		
		lineFit(&leftList, output);
		lineFit(&rightList, output);
	}
	output->push_back((*input)[input->size()-1]);
	
	return 0;
}

template <typename T> double	lineLength(CVec2d<typename T>* p1, CVec2d<typename T>* p2)
{
	double x = (double)((*p1)[0] - (*p2)[0]);
	double y = (double)((*p1)[1] - (*p2)[1]);
	return sqrt((x*x) + (y*y));	
}

#endif